package drone.iqgc.common ;

import java.util.Collections ;
import java.util.HashMap ;
import java.util.Map ;
import java.util.concurrent.TimeUnit ;

import javax.annotation.PostConstruct ;

import org.slf4j.Logger ;
import org.slf4j.LoggerFactory ;
import org.springframework.beans.factory.annotation.Autowired ;
import org.springframework.stereotype.Component ;

import drone.iqgc.service.MessageService ;
import io.mavsdk.System ;
import io.mavsdk.mission.Mission ;
import io.mavsdk.telemetry.Telemetry.FlightMode ;
import io.mavsdk.telemetry.Telemetry.Health ;
import io.mavsdk.telemetry.Telemetry.Position ;
import io.reactivex.schedulers.Schedulers ;

@Component
public class DroneManager
{

	public static final int statusDeley = 30 ;

	public static final int positionDeley = 3 ;

	public static final int flightModeDeley = 10 ;

	public static final int healthDeley = 30 ;

	public static final int batteryDeley = 60 ;

	public static final int landedStateDeley = 10 ;

	public static final int vtolStateDeley = 10 ;

	public static final Logger logger = LoggerFactory.getLogger ( DroneManager.class ) ;

	public Map < String , System > drones = Collections.synchronizedMap ( new HashMap < String , System > ( ) ) ;

	@Autowired
	public MessageService messageService ;

	public String getStatus (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			return "1" ;
		}

		return "0" ;
	}

	public System getDrone (
			String id
	)
	{

		if ( drones.containsKey ( id ) )
		{
			return drones.get ( id ) ;
		}

		return null ;

	}

	@PostConstruct
	public void init ( ) throws Exception
	{

		for ( int i = 1 ; i < 5 ; i ++ )
		{
			final String id = String.format ( "%02d" , i ) ;

			System drone = new System ( "60.205.58.145" , Integer.parseInt ( "5004" + String.valueOf ( i ) ) ) ;

//			Health health = drone.getTelemetry ( ).getHealth ( ).blockingFirst ( ) ;
//
//			Boolean s1 = health.getIsAccelerometerCalibrationOk ( ) ;
//			Boolean s2 = health.getIsGlobalPositionOk ( ) ;
//			Boolean s3 = health.getIsGyrometerCalibrationOk ( ) ;
//			Boolean s4 = health.getIsHomePositionOk ( ) ;
//			Boolean s5 = health.getIsLocalPositionOk ( ) ;
//			Boolean s6 = health.getIsMagnetometerCalibrationOk ( ) ;
//
//			logger.info ( "getIsAccelerometerCalibrationOk : " + s1.toString ( ) ) ;
//			logger.info ( "getIsGyrometerCalibrationOk : " + s3.toString ( ) ) ;
//			logger.info ( "getIsMagnetometerCalibrationOk : " + s6.toString ( ) ) ;
//			logger.info ( "getIsGlobalPositionOk : " + s2.toString ( ) ) ;
//			logger.info ( "getIsHomePositionOk : " + s4.toString ( ) ) ;
//			logger.info ( "getIsLocalPositionOk : " + s5.toString ( ) ) ;
//
//			java.lang.System.out.println ( "getHealthAllOk : " + drone.getTelemetry ( ).getHealthAllOk ( ).blockingFirst ( ) ) ;

			drone.getAction ( ).arm ( ).retryWhen ( retry ->
			{
				return retry.doOnNext ( e ->
				{
					logger.error ( id + " - " + e.getMessage ( ) ) ;
				} ).delay ( statusDeley , TimeUnit.SECONDS ) ;

			} ).subscribeOn ( Schedulers.io ( ) ).subscribe ( ( ) ->
			{
				logger.info ( id + " 成功连接!" ) ;
				drones.put ( id , drone ) ;
				messageService.droneStatusMessage ( "1" , "操作成功" , id , "1" ) ;

				reportPosition ( id ) ;
				reportFlightMode ( id ) ;
				reportHealth ( id ) ;
				reportBattery ( id ) ;
				reportLandedState ( id ) ;
				reportVtolState ( id ) ;

			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;
		}

	}

	// 上报状态
	public void executeStatus (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			messageService.droneStatusMessage ( "1" , "操作成功" , id , "1" ) ;
		}
	}

	// 执行位置
	public void executePosition (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			Position position = drone.getTelemetry ( ).getPosition ( ).doOnError ( err ->
			{
				logger.info ( id + " position exception : " + err.getMessage ( ) ) ;

			} ).blockingFirst ( ) ;

			messageService.dronePositionMessage ( "1" , "操作成功" , id , position.getLongitudeDeg ( ) , position.getLatitudeDeg ( ) , position.getAbsoluteAltitudeM ( ) , position.getRelativeAltitudeM ( ) ) ;

		}
	}

	// 执行飞行模式
	public void executeFlightmode (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			FlightMode flightMode = drone.getTelemetry ( ).getFlightMode ( ).doOnError ( err ->
			{
				logger.info ( id + " flight exception : " + err.getMessage ( ) ) ;

			} ).blockingFirst ( ) ;

			messageService.droneFlightmodeMessage ( "1" , "操作成功" , id , flightMode.name ( ) ) ;

		}
	}

	// 执行飞行模式
	public void executeMaxspeed (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			Float maxspeed = drone.getAction ( ).getMaximumSpeed ( ).blockingGet ( ) ;

			messageService.droneMaxspeedMessage ( "1" , "操作成功" , id , maxspeed ) ;

		}
	}

	// 执行飞行模式
	public void executeHome (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			Position home = drone.getTelemetry ( ).getHome ( ).doOnError ( err ->
			{
				logger.info ( id + " home exception : " + err.getMessage ( ) ) ;

			} ).blockingFirst ( ) ;

			messageService.droneHomeMessage ( "1" , "操作成功" , id , home.getLongitudeDeg ( ) , home.getLatitudeDeg ( ) , home.getAbsoluteAltitudeM ( ) , home.getRelativeAltitudeM ( ) ) ;

		}
	}

	public void executeGoto (
			String id ,
			Float speed ,
			Double lng ,
			Double lat
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			Position position = drone.getTelemetry ( ).getPosition ( ).doOnError ( err ->
			{
				logger.info ( "position exception : " + err.getMessage ( ) ) ;

			} ).blockingFirst ( ) ;

			float abs = position.getAbsoluteAltitudeM ( ).floatValue ( ) ;
			float rel = position.getRelativeAltitudeM ( ).floatValue ( ) ;

			drone.getAction ( ).arm ( ).doOnComplete ( ( ) ->
			{
				logger.info ( id + " goto drone arm complate " ) ;

			} ).doOnError ( err ->
			{
				logger.info ( id + " goto drone arm exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ).andThen ( drone.getAction ( ).takeoff ( ).doOnComplete ( ( ) ->
			{
				logger.info ( id + " goto drone takeoff complate " ) ;

			} ).doOnError ( err ->
			{
				logger.info ( id + " goto drone takeoff exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ).andThen (

					drone.getAction ( ).setCurrentSpeed ( speed ).andThen ( drone.getAction ( ).gotoLocation ( lat , lng , abs , rel ).doOnError ( err ->
					{
						logger.info ( id + " goto exception : " + err.getMessage ( ) ) ;

					} ) ) ) ).subscribe ( ( ) ->
					{
						messageService.droneGotoMessage ( "1" , "操作成功" , id ) ;

					} , throwable ->
					{
						logger.error ( id + throwable.getMessage ( ) ) ;

					} ) ;

		}
	}

	public void executeTakeoff (
			String id ,
			Float alt
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getAction ( ).arm ( ).andThen ( drone.getAction ( ).setTakeoffAltitude ( alt ).andThen ( drone.getAction ( ).takeoff ( ).doOnError ( err ->
			{
				logger.info ( id + " takeoff exception : " + err.getMessage ( ) ) ;

			} ) ) ).subscribe ( ( ) ->
			{
				messageService.droneTakeoffMessage ( "1" , "操作成功" , id ) ;

			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;

			} ) ;

		}
	}

	public void executeLand (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getAction ( ).arm ( ).andThen ( drone.getAction ( ).land ( ).doOnError ( err ->
			{
				logger.info ( id + " land exception : " + err.getMessage ( ) ) ;

			} ) ).subscribe ( ( ) ->
			{
				messageService.droneLandMessage ( "1" , "操作成功" , id ) ;

			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;

			} ) ;

		}
	}

	public void executeReturn (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getAction ( ).arm ( ).andThen ( drone.getAction ( ).returnToLaunch ( ).doOnError ( err ->
			{
				logger.info ( id + " return exception : " + err.getMessage ( ) ) ;

			} ) ).subscribe ( ( ) ->
			{
				messageService.droneReturnMessage ( "1" , "操作成功" , id ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	public void executeAltitude (
			String id ,
			Float alt
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			Position position = drone.getTelemetry ( ).getPosition ( ).blockingFirst ( ) ;

			double lat = position.getLatitudeDeg ( ) ;
			double lng = position.getLongitudeDeg ( ) ;
			float abs = position.getAbsoluteAltitudeM ( ).floatValue ( ) + alt.floatValue ( ) ;
			float rel = position.getRelativeAltitudeM ( ).floatValue ( ) + alt.floatValue ( ) ;

			drone.getAction ( ).arm ( ).andThen ( drone.getAction ( ).gotoLocation ( lat , lng , abs , rel ).doOnError ( err ->
			{
				logger.info ( id + " goto altitue exception : " + err.getMessage ( ) ) ;

			} ) ).subscribe ( ( ) ->
			{
				messageService.droneAltitudeMessage ( "1" , "操作成功" , id ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	public void executeCamera (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			// drone.getCamera ( ).getCaptureInfo ( ).subscribe ( captureInfo ->
			// logger.info ( "Picture taken: " + captureInfo.getFileUrl ( ) ) )
			// ;

			drone.getCamera ( ).takePhoto ( ).doOnError ( err ->
			{
				logger.info ( id + " camera exception : " + err.getMessage ( ) ) ;
				messageService.droneCameraMessage ( "0" , err.getMessage ( ) , id ) ;

			} ).subscribe ( ( ) ->
			{
				messageService.droneCameraMessage ( "1" , "操作成功" , id ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	public void executeMissionUpload (
			String id ,
			Boolean home ,
			Mission.MissionPlan missionPlan
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getMission ( ).setReturnToLaunchAfterMission ( home ).andThen ( drone.getMission ( ).uploadMissionWithProgress ( missionPlan ).doOnError ( err ->
			{
				logger.info ( id + " mission upload exception : " + err.getMessage ( ) ) ;

			} ).map ( mission ->
			{
				return mission.getProgress ( ) ;

			} ).distinctUntilChanged ( ) ).subscribe ( percent ->
			{
				messageService.droneMissionUploadMessage ( "1" , "操作成功" , id , String.valueOf ( percent ) ) ;
				logger.info ( id + " 上传进度 : " + String.valueOf ( percent ) ) ;

			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;

			} ) ;

		}
	}

	public void executeMissionDownload (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getMission ( ).downloadMission ( ).doOnError ( err ->
			{
				logger.info ( id + " mission download exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionDownloadMessage ( "0" , err.getMessage ( ) , id , null ) ;

			} ).subscribe ( missionPlan ->
			{
				logger.info ( id + " 任务下载成功 节点 " + missionPlan.getMissionItems ( ).size ( ) + " 个 " ) ;
				messageService.droneMissionDownloadMessage ( "1" , "操作成功" , id , missionPlan ) ;

			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	public void executeMissionStart (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			Health health = drone.getTelemetry ( ).getHealth ( ).blockingFirst ( ) ;

			if ( ! health.getIsGlobalPositionOk ( ) )
			{
				logger.info ( "getIsGlobalPositionOk :" + health.getIsGlobalPositionOk ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , "getIsGlobalPositionOk" , id , "" ) ;
				return ;
			}

			if ( ! health.getIsArmable ( ) )
			{
				logger.info ( "getIsArmable :" + health.getIsArmable ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , "getIsArmable" , id , "" ) ;
				return ;
			}

			if ( ! health.getIsHomePositionOk ( ) )
			{
				logger.info ( "getIsHomePositionOk :" + health.getIsHomePositionOk ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , "getIsHomePositionOk" , id , "" ) ;
				return ;
			}

			if ( ! health.getIsLocalPositionOk ( ) )
			{
				logger.info ( "getIsLocalPositionOk :" + health.getIsLocalPositionOk ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , "getIsLocalPositionOk" , id , "" ) ;
				return ;
			}

			if ( ! health.getIsAccelerometerCalibrationOk ( ) )
			{
				logger.info ( "getIsAccelerometerCalibrationOk :" + health.getIsAccelerometerCalibrationOk ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , "getIsAccelerometerCalibrationOk" , id , "" ) ;
				return ;
			}

			drone.getAction ( ).arm ( ).doOnComplete ( ( ) ->
			{
				logger.info ( id + " mission start drone arm complate " ) ;

			} ).doOnError ( err ->
			{
				logger.info ( id + " mission start drone arm exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ).andThen ( drone.getAction ( ).takeoff ( ).doOnComplete ( ( ) ->
			{
				logger.info ( id + " mission start drone takeoff complate " ) ;

			} ).doOnError ( err ->
			{
				logger.info ( id + " mission start drone takeoff exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ).andThen ( drone.getMission ( ).startMission ( ).doOnComplete ( ( ) ->
			{
				logger.info ( id + " mission start complate " ) ;
				messageService.droneMissionStartMessage ( "1" , "操作成功" , id , "0.00" ) ;

			} ).doOnError ( err ->
			{
				logger.info ( id + " mission start exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ).andThen ( drone.getMission ( ).getMissionProgress ( ).doOnError ( err ->
			{
				logger.info ( id + " mission start progress exception : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ).map ( mission ->
			{
				logger.info ( id + " 任务当前 : " + mission.getCurrent ( ) + " 任务总共 : " + mission.getTotal ( ) ) ;
				return mission.getCurrent ( ).doubleValue ( ) / mission.getTotal ( ).doubleValue ( ) ;

			} ) ) ).distinctUntilChanged ( ) ).subscribe ( percent ->
			{
				logger.info ( id + " 任务执行进度 : " + String.valueOf ( percent ) ) ;
				messageService.droneMissionStartMessage ( "1" , "操作成功" , id , String.valueOf ( percent ) ) ;

			} , err ->
			{
				logger.error ( id + " 任务执行异常 : " + err.getMessage ( ) ) ;
				messageService.droneMissionStartMessage ( "0" , err.getMessage ( ) , id , "" ) ;

			} ) ;

		}
	}

	public void executeMissionCanel (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getMission ( ).clearMission ( ).doOnError ( err ->
			{
				logger.info ( id + " mission canel exception : " + err.getMessage ( ) ) ;

			} ).subscribe ( ( ) ->
			{
				messageService.droneMissionCanelMessage ( "1" , "操作成功" , id ) ;
				logger.info ( id + " 任务撤销成功 " ) ;

			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;

			} ) ;

		}
	}

	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////

	// 上报位置
	public void reportPosition (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getTelemetry ( ).getPosition ( ).throttleFirst ( positionDeley , TimeUnit.SECONDS ).subscribe ( position ->
			{

				messageService.dronePositionMessage ( "1" , "操作成功" , id , position.getLongitudeDeg ( ) , position.getLatitudeDeg ( ) , position.getAbsoluteAltitudeM ( ) , position.getRelativeAltitudeM ( ) ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	// 上报飞行模式
	public void reportFlightMode (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getTelemetry ( ).getFlightMode ( ).throttleFirst ( flightModeDeley , TimeUnit.SECONDS ).subscribe ( flightMode ->
			{

				messageService.droneFlightmodeMessage ( "1" , "操作成功" , id , flightMode.name ( ) ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	// 上报健康状态
	public void reportHealth (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getTelemetry ( ).getHealth ( ).throttleFirst ( healthDeley , TimeUnit.SECONDS ).subscribe ( health ->
			{

				messageService.dronHealthMessage ( "1" , "操作成功" , id , health.getIsHomePositionOk ( ) ? "1" : "0" , health.getIsArmable ( ) ? "1" : "0" ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	// 上报电池
	public void reportBattery (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getTelemetry ( ).getBattery ( ).throttleFirst ( batteryDeley , TimeUnit.SECONDS ).subscribe ( battery ->
			{

				messageService.droneBatteryMessage ( "1" , "操作成功" , id , String.valueOf ( battery.getRemainingPercent ( ) ) ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	// 上报land状态
	public void reportLandedState (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getTelemetry ( ).getLandedState ( ).throttleFirst ( landedStateDeley , TimeUnit.SECONDS ).subscribe ( landedState ->
			{

				messageService.droneLandedStateMessage ( "1" , "操作成功" , id , landedState.name ( ) ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	// 上报飞行状态
	public void reportVtolState (
			String id
	)
	{
		if ( drones.containsKey ( id ) )
		{
			System drone = drones.get ( id ) ;

			drone.getTelemetry ( ).getVtolState ( ).throttleFirst ( vtolStateDeley , TimeUnit.SECONDS ).subscribe ( vtolState ->
			{

				messageService.droneVtolStateMessage ( "1" , "操作成功" , id , vtolState.name ( ) ) ;
			} , throwable ->
			{
				logger.error ( id + throwable.getMessage ( ) ) ;
			} ) ;

		}
	}

	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////////

	public Map < String , System > getDrones ( )
	{
		return drones ;
	}

	public void setDrones (
			Map < String , System > drones
	)
	{
		this.drones = drones ;
	}

}
